using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class GraphNode
{
    public const int INVAILD_NODE_INDEX = -1;
    public int index;
    public Vector3 position;

    public GraphNode(int index, Vector3 pos)
    {
        this.index = index;
        position = pos;
    }
}

public class GraphEdge
{
    public int from;
    public int to;
    public float cost;

    public GraphEdge(int from, int to, float cost)
    {
        this.from = from;
        this.to = to;
        this.cost = cost;
    }

    public GraphEdge(int from, int to)
    {
        this.from = from;
        this.to = to;
        this.cost = 1.0f;
    }
}

public class Graph
{
    private List<GraphNode> _nodes;
    private List<List<GraphEdge>> _edges;
    private bool _isDigraph;
    private int _nextNodeIndex;


    public Graph(bool isDigraph)
    {
        _nodes = new List<GraphNode>();
        _edges = new List<List<GraphEdge>>();
        _isDigraph = isDigraph;
        _nextNodeIndex = 0;
    }

    public List<GraphNode> GetNodes()
    {
        return _nodes;
    }

    public GraphNode GetNode(int index)
    {
        return _nodes[index];
    }

    public GraphEdge GetEdge(int from, int to)
    {
        if (_edges.Count <= from)
        {
            return null;
        }

        foreach (var edge in _edges[from])
        {
            if (edge.to == to)
            {
                return edge;
            }
        }

        return null;
    }

    public List<GraphEdge> GetEdges(int nodeIndex)
    {
        if (nodeIndex >= _edges.Count || nodeIndex < 0)
        {
            throw new Exception();
        }

        return _edges[nodeIndex];
    }

    public int AddNode(GraphNode node)
    {
        if (node.index < _nodes.Count)
        {
            if (_nodes[node.index].index != GraphNode.INVAILD_NODE_INDEX)
            {
                throw new Exception();
            }

            _nodes[node.index] = node;
            return _nextNodeIndex;
        }
        else
        {
            if (node.index != _nextNodeIndex)
            {
                throw new Exception();
            }

            _nodes.Add(node);
            _edges.Add(new List<GraphEdge>());
            return ++_nextNodeIndex;
        }
    }

    public int GetNextFreeNodeIndex()
    {
        return _nextNodeIndex;
    }

    public void CLear()
    {
        _nextNodeIndex = 0;
        _edges.Clear();
        _nodes.Clear();
    }

    public int NodeCount()
    {
        return _nodes.Count;
    }


    public void RemoveNode(int node)
    {
        _nodes[node].index = GraphNode.INVAILD_NODE_INDEX;

        if (!_isDigraph)
        {
            foreach (var edge in _edges[node])
            {
                foreach (var toEdge in _edges[edge.to])
                {
                    if (toEdge.to == node)
                    {
                        _edges[edge.to].Remove(toEdge);
                        break;
                    }
                }
            }

            _edges[node].Clear();
        }
        else
        {
            CullInvalidEdges();
        }
    }

    public void AddEdge(GraphEdge edge)
    {
        if (edge.from >= _nextNodeIndex || edge.to >= _nextNodeIndex)
        {
            throw new Exception();
        }

        if (UniqueEdge(edge.from, edge.to))
        {
            _edges[edge.from].Add(edge);
        }

        if (!_isDigraph)
        {
            if (UniqueEdge(edge.to, edge.from))
            {
                GraphEdge newEdge = new GraphEdge(edge.to, edge.from);
                _edges[edge.to].Add(newEdge);
            }
        }
    }

    public void AddEdge(int from, int to)
    {
        GraphNode fromNode = _nodes[from];
        GraphNode toNode = _nodes[to];
        float cost = (fromNode.position - toNode.position).magnitude;
        AddEdge(new GraphEdge(from, to, cost));
    }

    public bool IsNodePresent(int index)
    {
        return index < _nodes.Count && index != GraphNode.INVAILD_NODE_INDEX;
    }

    public bool IsEdgePresent(int from, int to)
    {
        if (!IsNodePresent(from) || !IsNodePresent(to))
        {
            return false;
        }

        foreach (var edge in _edges[from])
        {
            if (edge.to == to)
            {
                return true;
            }
        }

        return false;
    }

    public void RemoveEdge(int from, int to)
    {
        if (from >= _nodes.Count || to >= _nodes.Count)
        {
            throw new Exception();
        }

        if (!_isDigraph)
        {
            for (int i = 0; i < _edges[to].Count; i++)
            {
                GraphEdge edge = _edges[to][i];
                if (edge.to == from)
                {
                    _edges[to].RemoveAt(i);
                    break;
                }
            }
        }

        for (int i = 0; i < _edges[from].Count; i++)
        {
            GraphEdge edge = _edges[from][i];
            if (edge.to == to)
            {
                _edges[from].RemoveAt(i);
                break;
            }
        }
    }

    private void CullInvalidEdges()
    {
        foreach (var edges in _edges)
        {
            for (int i = edges.Count; i >= 0; i++)
            {
                GraphEdge edge = edges[i];
                if (_nodes[edge.to].index == GraphNode.INVAILD_NODE_INDEX ||
                    _nodes[edge.from].index == GraphNode.INVAILD_NODE_INDEX)
                {
                    edges.RemoveAt(i);
                }
            }
        }
    }

    private bool UniqueEdge(int from, int to)
    {
        return GetEdge(from, to) == null;
    }

    public IEnumerator EdgeIterable(int index)
    {
        return _edges[index].GetEnumerator();
    }
}

public class GraphSearch
{
    private const int VISITED = 0;
    private const int UNVISITED = 1;
    private const int NO_PARENT_ASSIGNED = -1;

    private Graph _graph;

    private List<int> _visited;
    private List<int> _route;
    private int _source;
    private int _target;
    private bool _isFound;
    private List<GraphEdge> _spanningTree;

    List<GraphEdge> _shortestPathTree;
    List<GraphEdge> _searchFrontier;

    public GraphSearch(Graph graph)
    {
        _graph = graph;
        _spanningTree = new List<GraphEdge>();
    }

    public List<GraphEdge> GetSpanningTree()
    {
        if (_searchFrontier != null && _searchFrontier.Count > 0)
        {
            return _searchFrontier;
        }

        return _spanningTree;
    }

    public bool SearchDFS(int source, int target)
    {
        _isFound = false;
        _source = source;
        _target = target;
        _spanningTree.Clear();
        _visited = new List<int>(_graph.NodeCount());
        _route = new List<int>(_graph.NodeCount());
        for (int i = 0; i < _graph.NodeCount(); i++)
        {
            _visited.Add(UNVISITED);
            _route.Add(NO_PARENT_ASSIGNED);
        }

        Stack<GraphEdge> stack = new Stack<GraphEdge>();
        GraphEdge dummy = new GraphEdge(_source, _source);
        stack.Push(dummy);

        while (stack.Count > 0)
        {
            GraphEdge next = stack.Pop();
            _route[next.to] = next.from;
            _visited[next.to] = VISITED;
            if (dummy != next)
            {
                _spanningTree.Add(next);
            }

            if (next.to == _target)
            {
                _isFound = true;
                return true;
            }

            IEnumerator it = _graph.EdgeIterable(next.to);
            while (it.MoveNext())
            {
                GraphEdge gE = it.Current as GraphEdge;
                if (_visited[gE.to] == UNVISITED)
                {
                    stack.Push(gE);
                }
            }
        }

        return false;
    }

    public bool SearchBFS(int source, int target)
    {
        _isFound = false;
        _source = source;
        _target = target;
        _visited = new List<int>(_graph.NodeCount());
        _route = new List<int>(_graph.NodeCount());
        _spanningTree.Clear();
        for (int i = 0; i < _graph.NodeCount(); i++)
        {
            _visited.Add(UNVISITED);
            _route.Add(NO_PARENT_ASSIGNED);
        }

        Queue<GraphEdge> stack = new Queue<GraphEdge>();
        GraphEdge dummy = new GraphEdge(_source, _source);
        stack.Enqueue(dummy);
        _visited[dummy.to] = VISITED;

        while (stack.Count > 0)
        {
            GraphEdge next = stack.Dequeue();
            _route[next.to] = next.from;
            if (dummy != next)
            {
                _spanningTree.Add(next);
            }

            if (next.to == _target)
            {
                _isFound = true;
                return true;
            }

            IEnumerator it = _graph.EdgeIterable(next.to);
            while (it.MoveNext())
            {
                GraphEdge gE = it.Current as GraphEdge;
                if (_visited[gE.to] == UNVISITED)
                {
                    stack.Enqueue(gE);
                    _visited[gE.to] = VISITED;
                }
            }
        }

        return false;
    }

    public bool SearchDijkstra(int source, int target)
    {
        return SearchDijkstra(source, target, null);
    }

    public bool SearchAStar(int source, int target)
    {
        return SearchDijkstra(source, target, (pos1, pos2) => { return (pos1 - pos2).magnitude; });
    }

    public bool SearchDijkstra(int source, int target, Func<Vector3, Vector3, float> heuristic)
    {
        _isFound = false;
        _source = source;
        _target = target;

        List<float> fCosts = new List<float>(_graph.NodeCount());
        List<float> gCosts = new List<float>(_graph.NodeCount());

        _shortestPathTree = new List<GraphEdge>();
        _searchFrontier = new List<GraphEdge>();
        for (int i = 0; i < _graph.NodeCount(); i++)
        {
            gCosts.Add(0);
            fCosts.Add(0);
            _shortestPathTree.Add(null);
            _searchFrontier.Add(null);
        }

        IndexedPriority<float> pq =
            new IndexedPriority<float>(fCosts, Comparer<float>.Default, fCosts.Count);

        pq.Insert(source);

        while (!pq.Empty())
        {
            int nextClosestNode = pq.Pop();
            _shortestPathTree[nextClosestNode] = _searchFrontier[nextClosestNode];
            if (nextClosestNode == target)
            {
                _isFound = true;
                return _isFound;
            }

            IEnumerator it = _graph.EdgeIterable(nextClosestNode);
            while (it.MoveNext())
            {
                GraphEdge edge = it.Current as GraphEdge;
                float gCost = gCosts[nextClosestNode] + edge.cost;
                float hCost = heuristic != null
                    ? heuristic(_graph.GetNode(edge.to).position, _graph.GetNode(target).position)
                    : 0;

                if (_searchFrontier[edge.to] == null)
                {
                    gCosts[edge.to] = gCost;
                    fCosts[edge.to] = gCost + hCost;

                    pq.Insert(edge.to);

                    _searchFrontier[edge.to] = edge;
                }

                else if (gCost < gCosts[edge.to] && _shortestPathTree[edge.to] == null)
                {
                    gCosts[edge.to] = gCost;
                    fCosts[edge.to] = gCost + hCost;

                    pq.ChangePriority(edge.to);

                    _searchFrontier[edge.to] = edge;
                }
            }
        }

        return _isFound;
    }


    public List<int> GetPathToTarget()
    {
        List<int> path = new List<int>();
        if (!_isFound || _target < 0)
        {
            return path;
        }

        int nd = _target;
        path.Add(nd);

        if (_shortestPathTree != null && _shortestPathTree.Count > 0)
        {
            while (nd != _source && _shortestPathTree[nd] != null)
            {
                nd = _shortestPathTree[nd].from;
                path.Add(nd);
            }
        }
        else
        {
            while (nd != _source)
            {
                nd = _route[nd];
                path.Add(nd);
            }
        }


        return path;
    }
}